#include "ODN_ModelBit.h"
#include "ODN_GameMgr.h"

#include <cassert>

namespace ouden {

/*REMOVE
CModelBit::CModelBit(opal::Solid* pSolid, Ogre::Node* pNode)
 : m_pSolid(pSolid), m_pNode(pNode)
{
	//ctor

	assert(pSolid);
	assert(pNode);

}*/

CModelBit::CModelBit(opal::Solid* pSolid, Ogre::Bone* pBone)
 : m_pSolid(pSolid), m_pNode(pBone)
{
	//ctor

	assert(pSolid);
	assert(pBone);

    pBone->setManuallyControlled(true);

}

CModelBit::CModelBit(Ogre::Node* pBone, float boneRadius, CModelBit* parent, bool recursive)
 : m_pNode(pBone)
{
	//ctor

    // PRETTY CRAP, give decent polish
	//assert(pBone);

    //(dynamic_cast<Ogre::Bone*>(pBone))->setManuallyControlled(true);

    //relative TODO: this

    m_pParent = parent;

    if(parent) {

        Ogre::Vector3 boneStart = pBone->getParent()->_getDerivedPosition(); // TODO: check if getParent returns null
        Ogre::Vector3 boneMid = boneStart + pBone->getPosition()*0.5;
        Ogre::Vector3 boneVec = pBone->getPosition() - boneStart;
        //REMOVE Ogre::Quaternion quat = pBone->_getDerivedOrientation();

        opal::Simulator* sim = CGameMgr::getInstance()->getSimulator();
        m_pSolid = sim->createSolid();
        opal::Matrix44r transform;
        transform.translate(boneMid[0], boneMid[1], boneMid[2]); // CHECK: (toOpalVector(boneMid); and the rest
        transform.rotate(0, boneVec[0], boneVec[1], boneVec[2]); //CRAP doesn't work
        m_pSolid->setTransform(transform);

        //CRAP
        std::cout << "position1 " << boneMid[0] <<" "<< boneMid[1] << std::endl;
        std::cout << "position2 " << (m_pSolid->getPosition())[0] <<" "<< (m_pSolid->getPosition())[1] << std::endl;

        //REMOVE std::cout << "orientation1 " << quat.w <<" "<< quat.x <<" "<< quat.y <<" "<< quat.z<< std::endl;
        std::cout << "orientation2 " << (m_pSolid->getQuaternion())[0] <<" "<< (m_pSolid->getQuaternion())[1] << std::endl;

        opal::CapsuleShapeData capsule;
        capsule.radius = boneRadius;
        capsule.length = pBone->getPosition().length()-2*boneRadius;
        //capsule.contactGroup = 1;
        m_pSolid->addShape(capsule);

        m_pSolid->setStatic(false);

        // Set up joint between this and parent
        //TODO: fix
        if(parent->getSolid()!=NULL){
            m_pJoint = sim->createJoint();
            opal::JointData jointData;
            jointData.setType(opal::BALL_JOINT);
            jointData.solid1 = m_pSolid;
            jointData.solid0 = parent->getSolid();
            jointData.anchor = toOpalPoint(boneStart);
            jointData.axis[0].direction = opal::Vec3r(1.0, 0.0, 0.0); //TODO: fix
            jointData.axis[1].direction = opal::Vec3r(0.0, 0.0, 1.0); //TODO: fix
            jointData.axis[2].direction = opal::Vec3r(0.0, 1.0, 0.0); //TODO: fix

            std::cout <<"here"<<std::endl;
            m_pJoint->init(jointData);
            std::cout <<"here2"<<std::endl;
        }

    }
    else{

        Ogre::Vector3 pos = pBone->_getDerivedPosition();

        opal::Simulator* sim = CGameMgr::getInstance()->getSimulator();
        m_pSolid = sim->createSolid();
        //sim->setupContactGroup(1, false);
        opal::Matrix44r transform;
        transform.translate(pos[0], pos[1], pos[2]); // CHECK: (toOpalVector(boneMid); and the rest
        transform.rotate(0, 1, 0, 0); // CRAP doesn't work
        m_pSolid->setTransform(transform);

        opal::SphereShapeData sphere;
        sphere.radius = boneRadius/100000;
        //sphere.contactGroup = 1;
        m_pSolid->addShape(sphere);

        m_pSolid->setStatic(false);

        //CRAP
        Ogre::Quaternion quat = m_pNode->_getDerivedOrientation();
        std::cout << "orientation1111 " << quat.w <<" "<< quat.x <<" "<< quat.y <<" "<< quat.z<< std::endl;

    }

    if(recursive) {

        // Iterate through direct children
        for(int i = 0; i < m_pNode->numChildren(); i++) { //TODO: make sure proper version uses bone iterator
            pBone = m_pNode->getChild(i);
            children.push_back(new CModelBit(pBone,boneRadius,this));
        }

    }

}

CModelBit::~CModelBit()
{
	//dtor

}

void CModelBit::snapSolidToNode()
{




}

void CModelBit::blendSolidToNode()
{

    //TODO: move the solid to the node by a limited amount

    snapNodeToSolid();

}

void CModelBit::snapNodeToSolid()
{
    /*  Calls to this function must be made in the heirachical order, this should
        be managed by the CModelEntity class
    */
    // TODO: convert opal quat to ogre quat

    if(m_pParent){
        Ogre::Quaternion q = m_pNode->_getDerivedOrientation().UnitInverse() * toOgreQuaternion(m_pSolid->getQuaternion()) * relative;
        m_pNode->setOrientation(q);

    //CRAP
        //std::cout << "orientation " << q.x << std::endl;//(m_pSolid->getQuaternion())[0] << std::endl;

    }

    // Iterate through direct children
    for(int i = 0; i < children.size(); i++) { //TODO: make sure proper version uses iterator
        children[i]->snapNodeToSolid();
    }

}


} //namespace ouden
